#include <WProgram.h>
#include <math.h>

extern "C" {
#include "Venus634.h"
}

/* 0xa8 - navigation data */
#define MO_NAVDATA 0xa8
typedef struct {
 uint8_t fixmode; /* 0 = none; 1 = 2D, 2 = 3D; 3 = 3D+DGPS */
 uint8_t svnum; /* number of sats in fix */
 uint16_t gps_week; /* GPS Week Number */
 uint32_t gps_tow; /* Time of week, seconds, 1/100th resolution */
 int32_t lattitde; /* 1/1e-7 (???) degree, positive north */
 int32_t longitude; /* 1/1e-7 (???) degree, positive east */
 uint32_t elips_alt; /* meters, 1/100th */
 uint32_t sea_alt; /* meters, 1/100th */
 uint16_t gdop; /* 1/100th */
 uint16_t pdop; /* 1/100th */
 uint16_t hdop; /* 1/100th */
 uint16_t vdop; /* 1/100th */
 uint16_t tdop; /* 1/100th */
 int32_t ecef_x; /* meters, 1/100th */
 int32_t ecef_y; /* meters, 1/100th */
 int32_t ecef_z; /* meters, 1/100th */
 int32_t ecef_vx; /* meters, 1/100th */
 int32_t ecef_vy; /* meters, 1/100th */
 int32_t ecef_vz; /* meters, 1/100th */
} mo_navdata_t;

/* 0x80 - software version */
#define MO_SOFTVER 0x80
typedef struct {
 uint8_t type; /* 1 = system code */
 uint32_t kernel; /* XX maj min patch */
 uint32_t odm; /* XX maj min patch */
 uint32_t rev; /* XX yy mm dd */
} mo_softver_t;


mo_navdata_t* NavData;
mo_softver_t* SofData;




float polySides = 6;
float polyX[] = {4,4,-4,-4,-2,-2};
float polyY[] = {4,-4,-4,0,0,4};

//  The function will return YES if the point x,y is inside the polygon, or
//  NO if it is not.  If the point is exactly on the edge of the polygon,
//  then the function may return YES or NO.
//
//  Note that division by zero is avoided because the division is protected
//  by the "if" clause which surrounds it.


//===========================
bool pointInPolygon(float x, float y)
{

  int i;
  int j=polySides-1;
  bool  oddNodes = false;

  for (i=0; i<polySides; i++)
  {
    if ((polyY[i]< y && polyY[j]>=y)   ||   ((polyY[j]< y && polyY[i]>=y) &&  (polyX[i]<=x || polyX[j]<=x)))
    {
      oddNodes^=(polyX[i]+(y-polyY[i])/(polyY[j]-polyY[i])*(polyX[j]-polyX[i])<x);
    }
    j=i;
  }

  return oddNodes;
}

//===========================
float shortestDistance (float lon, float lat)
{
        int i;
        float Ax, Bx, Cx, Ay, By, Cy, a, b, c, r;
        float tempDistance;
        float distance = 999999999;


        //Iterate through sets of adjacent boundary points

        for(i=0; i<polySides; i++)
        {
                Ax = polyX[i];
                Ay = polyY[i];
                if(i==polySides-1)
                {
                        Bx = polyX[0];
                        By = polyY[0];
                }
                else
                {
                        Bx = polyX[i+1];
                        By = polyY[i+1];
                }
                Cx = lon;
                Cy = lat;


                //r represents where the perpendicular intersection of the test point intersects the line segment formed by
                //the two boundary points. If r<0 the intersection occurs outside of bounds1. If r>1 the intersection
                //occurs outside of bounds2. If 0<r<1 the intersection occurs on the line segment.

                r = ((Cx-Ax)*(Bx-Ax)+(Cy-Ay)*(By-Ay))/pow(sqrt(pow((Bx-Ax),2)+pow((By-Ay),2)),2);

                if (0<r && r<1)
                {

                         //This tests whether the given boundary line segment is a vertical line. If it is, we swap all x and y
                         //coordinates for both boundary points and the test point. This allows us to sidestep the vertical line
                         //problem and calculate the accurate distance.

                        if(Ax==Bx)
                        {
                                a = (Bx-Ax)/(By-Ay);
                                b = -1;
                                c = Ax - ((Bx-Ax)/(By-Ay))*Ay;
                                Cx = lat;
                                Cy = lon;
                        }
                        else
                        {
                                a = (By-Ay)/(Bx-Ax);
                                b = -1;
                                c = Ay - ((By-Ay)/(Bx-Ax))*Ax;
                        }


                         //The absolute value function was truncating our doubles to integers, so we implemented our own here.
                         //We use the point-line distance formula.

                        if(a*Cy+b*Cx+c >= 0)
                        {
                                tempDistance = (a*Cx+b*Cy+c)/sqrt(a*a+b*b);
                        }
                        else
                        {
                                tempDistance = -1*(a*Cx+b*Cy+c)/sqrt(a*a+b*b);
                        }


                         //We only want to consider positive distances- otherwise the smallest distance will be the greatest
                         //negative number

                        if(tempDistance < 0)
                        {
                                tempDistance *= -1;
                        }


                        if (distance>tempDistance)
                        {
                        	//return tempDistance;
                        }
                }

                 //If the perpendicular intersection occurs outside of the line segment, we calculate the distance to
                 //each of the two boundary points.

                else
                {
                        tempDistance = sqrt(pow((Cx-Ax),2)+pow((Cy-Ay),2));
                        if (distance>tempDistance)
                        {
                        	return tempDistance;
                        }

                        tempDistance = sqrt(pow((Cx-Bx),2)+pow((Cy-By),2));
                        if (distance>tempDistance)
                        {
                        	return tempDistance;
                        }
                }

        }
        return distance;
}




void setup(){
	init();
	pinMode(13, OUTPUT);
	Serial.begin(9600);

	venus634_init();

	digitalWrite(13, HIGH);

	delay(2000);

	digitalWrite(13, LOW);

	delay(2000);
}



void loop(){

	while(Serial.available()>0){
		if(venus634_process((uint8_t)Serial.read())){
			uint8_t* tempPkt = venus634_getPacket();
			if(tempPkt[5]==0xa8){
				NavData = (mo_navdata_t *)&tempPkt;
			}
			if(tempPkt[5]==0x80){
				SofData = (mo_softver_t *)&tempPkt;
			}
		}
	}
	int i;
	for(i=0; i<10; i++){

		float x1 = 8*rand();
		float y1 = 8*rand();
		if(pointInPolygon(x1,y1)){
			Serial.println("In bounds, dist = ");
			Serial.print(shortestDistance(x1, y1));
		}
		else{
			Serial.println("Out of bounds, dist = ");
			Serial.print(shortestDistance(x1, y1));
		}
	}
}
